#ifndef OBSTACLES_H
#define OBSTACLES_H

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include <vector>
#include <utility>
#include <iostream>
#include <cmath>
#include <tf/tf.h>
#include <algorithm> 


// 定义PI常量
const double PI = 3.14159265358979323846;


// 定义Obstacle结构体
struct myObstacle {
    double x;
    double y;

    myObstacle(double x, double y) : x(x), y(y) {}
};

// 定义Obstacles类
class Obstacles {
public:
    // 构造函数和析构函数
    Obstacles();
    ~Obstacles();

    // 激光数据处理函数
    void processLaserData(const sensor_msgs::LaserScan::ConstPtr& scan);

    // 获取odom数据并更新车辆位置和姿态
    void odomCallback(const nav_msgs::Odometry::ConstPtr& odom);

    // 将障碍物坐标转换到地图坐标
    void transformToMapCoordinates(double x, double y);


    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);

    std::vector<myObstacle> getObstaclesAsPointers();

    void visualizeObstacles();  // 可视化障碍物

private:
    // 车辆的位置和姿态
    double vehicle_x, vehicle_y, vehicle_yaw;

    // 障碍物坐标列表
    std::vector<std::pair<double, double>> obstacles;
    std::vector<std::pair<double, double>> map_coordinates;

    // 激光数据订阅者
    ros::Subscriber laser_;

    // odom数据订阅者
    ros::Subscriber odom_;

    ros::Publisher marker_pub_;

};

#endif // OBSTACLES_H

